#include <Ethernet.h>
#include <SPI.h>
#include "Arduino.h"
#include "arduinoPins.h"

#define MTR_A true
#define MTR_B false

void rotate(int steps, float speed, bool mtr);

void setup()
{
  pinMode(BUTTON_A, INPUT);
  pinMode(BUTTON_B, INPUT);

  pinMode(SEL_A, OUTPUT);//mux select lines
  pinMode(SEL_B, OUTPUT);

  pinMode(DIR_PIN_A, OUTPUT);
  pinMode(STEP_PIN_A, OUTPUT);

  pinMode(DIR_PIN_B, OUTPUT);
  pinMode(STEP_PIN_B, OUTPUT);

}

void loop()
{
  digitalWrite(MAGNET, HIGH);

  //Moves both motors in parallel around the same speed until they both hit the edge
  digitalWrite(DIR_PIN_A, HIGH);
  digitalWrite(DIR_PIN_B, HIGH);
  //stop when both buttons are pressed
  while((digitalRead(BUTTON_A) == HIGH) || (digitalRead(BUTTON_B) == HIGH)) {
    if(digitalRead(BUTTON_A) == HIGH) //If white motor presses button, stop it
      digitalWrite(STEP_PIN_A, HIGH);
    if(digitalRead(BUTTON_B) == HIGH) { //gray motor needs to move at less delay
      digitalWrite(STEP_PIN_B, HIGH);
      digitalWrite(STEP_PIN_B, LOW);
      digitalWrite(STEP_PIN_B, HIGH);
    }
    delayMicroseconds(120);
    digitalWrite(STEP_PIN_A, LOW);
    digitalWrite(STEP_PIN_B, LOW);
    delayMicroseconds(120);
  }

  //Go to the 0, 0 square
  rotate(1890 * -5, .6, MTR_A);
  rotate(3700 * -4.5, 1, MTR_B);
  delay(1000);

  //move out across the board
  //Was getting an overflow error when moving by 12+ cm in one rotate
  rotate(1900 * -6, .6, MTR_A); //Rotate 6 centimeters
  rotate(1900 * -6, .8, MTR_A); //Rotate 6 centimeters
  rotate(1900 * -6, 1, MTR_A); //Rotate 6 centimeters
  rotate(1900 * -6, 1, MTR_A); //Rotate 6 centimeters
  rotate(3750 * -6, 1, MTR_B); //Rotate 6 centimeters
  rotate(3750 * -6, 1.2, MTR_B); //Rotate 6 centimeters
  rotate(3750 * -6, 1.5, MTR_B); //Rotate 6 centimeters
  rotate(3750 * -6, 1.8, MTR_B); //Rotate 6 centimeters

  delay(1000);
  digitalWrite(MAGNET, LOW);
}

void rotate(int steps, float speed, bool mtr) {
  //rotate a specific number of microsteps (8 microsteps per step) - (negative for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger
  int dir = (steps > 0)? HIGH:LOW;
  int dir_pin = (mtr) ? DIR_PIN_A : DIR_PIN_B;
  int step_pin = (mtr) ? STEP_PIN_A : STEP_PIN_B;
  steps = abs(steps);

  digitalWrite(dir_pin, dir);

  //limit the white motor's speed since it's already faster
  if(mtr && speed > 1) {
    speed = 1;
  }

  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){
    digitalWrite(step_pin, HIGH);
    delayMicroseconds(usDelay); 

    digitalWrite(step_pin, LOW);
    delayMicroseconds(usDelay);
  }
}





